/**
 * @file   MiraP3Arm.cpp
 * @author Josep Sabater (jsabaterm@el.ub.edu)
 * @date   November, 2016
 * @version 1.0.0
 * @ingroup QMira
 * @brief  Miradas P3 Probe Arm
 *
 */

#include <cmath>
#include <cassert>
#include <qt4/QtGui/qmatrix.h>

#include "MiraMath.h"
#include "MiraP3Arm.h"

namespace QMira{
    
    IRobot2D::IRobot2DId_t P3Arm::idCntr_ = 0;
    
    P3Arm::P3Arm(const MiraGeometryLib::Placement2D &origFrame,
                 const P3Arm::Lengths &aLen) 
            : id_(idCntr_++)
            , status_(IRobot2D::Status::AVAILABLE)
            , j1PosWorld_(origFrame.pos) 
            , j2PosWorld_(initJ2World_(origFrame, aLen))
            , lengths_(aLen) {
                
        /*
         * Qt, measures the angles counter-clockwise related to a
         * point in the X axis located in x>0, i.e.: "." (Alpha)
         * However, Qt's coordinate system is NOT Cartesian, because
         * in Qt y>0 is below the X axis (the Y axis is flipped)
         * See: http://doc.qt.io/qt-4.8/coordsys.html
         *
         * If we want to get the angle in the cartesian space, we
         * need to do some conversion
         *
         *                  ^ +y
         *          J1 *    |
         *              \   |
         *               \ _|_
         *                \ | \ Alpha
         *          Beta / \| |
         * -x<--------------+---------.-----> +x
         *                 /|
         *                / |
         *               /  |
         *              /   |
         *       J1_Qt *    |
         *
         *
         *  Multiplying the Alpha by "-1", flips it over Y axis
         *  and makes the mapping from Qt coordinates to Cartesian.
         *  Additionally, if we are interested in Beta instead
         *  we need to apply an offset of 180
         */  
        
        /* Alpha = 360-orient*/
        const double alpha = 360. - (origFrame.orient);

        /* thetaOffset_ = Beta in radians */               
        thetaOffset_ = M_PI - MiraUtilUBLib::Math::degreeToRadian(alpha);
        
        /* Determine transformation Matrix */
        transRobot2World_.reset();
        transRobot2World_.translate(j1PosWorld_.x(), j1PosWorld_.y());
        transRobot2World_.rotate( alpha ); // This is in degrees!!!!
    }
    
    
    IRobot2D::IRobot2DId_t P3Arm::id() const { return id_; }           
    IRobot2D::Status P3Arm::status() const { return status_; }                     
    void P3Arm::status(const IRobot2D::Status &st) { status_ = st; }
        
    MiraUtilUBLib::DoubleCont_t P3Arm::getQs() const { 
        return MiraUtilUBLib::DoubleCont_t ();
    };

    Joint2D::Joint2DCont_t P3Arm::getJoints(const MiraUtilUBLib::DoubleCont_t &q) const {
        /* Check that the incoming configuration has 2 elements*/
        assert(q.size() == 2);
        
        /* values are returned in degrees */
        const double th1=-q[0], th2=-q[1];
        return {Joint2D("J1", whereIsJ1_(th1)), 
                Joint2D("J2", whereIsJ2_(th2)), 
                Joint2D("J3", whereIsJ3_(th1)), 
                Joint2D("J4", whereIsJ4_(th2))};
    }
        
        
    const MiraUtilUBLib::DoubleCont_t& P3Arm::getLinksLength() const { 
        static const MiraUtilUBLib::DoubleCont_t l = { lengths_.dL1_, 
                                                       lengths_.dL2_, 
                                                       lengths_.dL3_, 
                                                       lengths_.dL4_};    
        return l;
    };        
        
    MiraGeometryLib::Vector2D P3Arm::robot2World(const MiraGeometryLib::Vector2D &aPoint) const {
        return MiraGeometryLib::Vector2D();
    }


    MiraGeometryLib::Vector2D P3Arm::world2Robot(const MiraGeometryLib::Vector2D &wPoint) const {
        return MiraGeometryLib::Vector2D();
    }

    std::string P3Arm::str() const { return std::string("NotImplemented"); }    
    
    
    MiraGeometryLib::Vector2D P3Arm::initJ2World_(const MiraGeometryLib::Placement2D &j1PosWorld,
                                                  const P3Arm::Lengths &lens) const {
        
        /* Position according to the local frame */
        MiraGeometryLib::Vector2D j2PosLocal(lens.dL4_, 0);
        double orientLocFrame = MiraUtilUBLib::Math::degreeToRadian(j1PosWorld.orient);
        
        /* Determine J2 absolute position */
        return (j1PosWorld_ - j2PosLocal.rotate(orientLocFrame));       
    }
    
    
    /* incoming value in radians, while outcome is in degrees */
    MiraGeometryLib::Placement2D P3Arm::whereIsJ1_(const double theta1) const {
        double oriDeg = MiraUtilUBLib::Math::radianToDegree(thetaOffset_+theta1-M_PI_2);
        return MiraGeometryLib::Placement2D(j1PosWorld_, oriDeg);        
    }
    
     /* incoming value in radians, while outcome is in degrees */
    MiraGeometryLib::Placement2D P3Arm::whereIsJ2_(const double theta2) const {
        double oriDeg = MiraUtilUBLib::Math::radianToDegree(thetaOffset_+theta2-M_PI_2);
        return MiraGeometryLib::Placement2D(j2PosWorld_, oriDeg);                 
    }
        
     /* incoming value in radians, while outcome is in degrees */
    MiraGeometryLib::Placement2D P3Arm::whereIsJ3_(const double theta1) const {
        const double d13 = lengths_.dL1_;
        MiraGeometryLib::Vector2D j3Local(d13*cos(theta1+thetaOffset_), d13*sin(theta1+thetaOffset_));
        return MiraGeometryLib::Placement2D(j1PosWorld_+ j3Local, 0);      
    }
                                                                 
    /* incoming value in radians, while outcome is in degrees */
    MiraGeometryLib::Placement2D P3Arm::whereIsJ4_(const double theta2) const {
        const double d24 = lengths_.dL2_;
        MiraGeometryLib::Vector2D j4Local(-d24*cos(theta2-thetaOffset_), d24*sin(theta2-thetaOffset_));
        return MiraGeometryLib::Placement2D(j2PosWorld_ + j4Local, 0);        
    }   
}
